#include "Plot/Plot.hpp"

plot::plot(rclcpp::Node::SharedPtr node) :
    node_(node)
{
    // 创建double数组的发布者
    array_pub_ = node_->create_publisher<std_msgs::msg::Float64MultiArray>("double_array_topic", 10);
    Timer_01 = node_->create_wall_timer(100ms, std::bind(&plot::Timer_Callback_01, this));
}

void plot::Timer_Callback_01()
{
    // 生成动态数组（例如随机波动的数据）
    for (size_t i = 0; i < array_value_.size(); ++i)
    {
        array_value_[i] += (rand() % 21 - 10) * 0.1;         // [-1.0, 1.0]随机波动
        array_value_[i] = clamp(array_value_[i], 0.0, 10.0); // 限制范围
    }

    // 发布double数组消息
    auto array_msg = std_msgs::msg::Float64MultiArray();
    array_msg.data = array_value_;
    array_pub_->publish(array_msg);

    // 优化日志输出（使用循环避免硬编码）
    std::stringstream ss;
    ss << "发布数组: [";
    for (size_t i = 0; i < array_value_.size(); ++i)
    {
        ss << std::fixed << std::setprecision(3) << array_value_[i];
        if (i < array_value_.size() - 1) ss << ", ";
    }
    ss << "]";
    RCLCPP_INFO(node_->get_logger(), "%s", ss.str().c_str());
}
// 实现该方法
void plot::publishDoubleArray(const std::vector<double> &data)
{
    // 发布double数组消息
    auto array_msg = std_msgs::msg::Float64MultiArray();
    array_msg.data = data;
    array_pub_->publish(array_msg);

    // 优化日志输出
    std::stringstream ss;
    ss << "发布数组: [";
    for (size_t i = 0; i < data.size(); ++i)
    {
        ss << std::fixed << std::setprecision(3) << data[i];
        if (i < data.size() - 1) ss << ", ";
    }
    ss << "]";
    RCLCPP_INFO(node_->get_logger(), "%s", ss.str().c_str());
}